% InvPositionSov.m

function res = InvPositionSov(rc, initTheta)
% rc C点坐标
% initTheta = [theta1_0, theta2_0, theta3_0, theta4_0] theta1~4的初值
% 返回值res = [theta1, theta2, theta3, theta4] theta1~4的值
    load("ConstantDefinition.mat", "l0", "theta0", "l1", "l2", "l3", "l4");
    theta1_0 = initTheta(1);
    theta2_0 = initTheta(2);
    theta3_0 = initTheta(3);
    theta4_0 = initTheta(4);

    opt=optimset('Display','off');    % 关闭输出

    fun = @(x)Five_Link_Inv([l0, l1, l2, l3, l4], [theta0, x(1), x(2), x(3), x(4)], rc);
    [y,~,exitflag,output,~] = fsolve(fun, [theta1_0, theta2_0, theta3_0, theta4_0], opt);
    if exitflag <= 0
        disp('fsolve failed');
        disp(output);
    end
    res = y;
end

function res = Five_Link_Inv(l, theta, rc)
    l0 = l(1);
    l1 = l(2);
    l2 = l(3);
    l3 = l(4);
    l4 = l(5);
    theta0 = theta(1);
    theta1 = theta(2);
    theta2 = theta(3);
    theta3 = theta(4);
    theta4 = theta(5);
    xc = rc(1);
    yc = rc(2);
    res(1) = l1*cos(theta1) + l2*cos(theta2) - xc;
    res(2) = l1*sin(theta1) + l2*sin(theta2) - yc;
    res(3) = l0*cos(theta0) + l3*cos(theta3) + l4*cos(theta4) - xc;
    res(4) = l0*sin(theta0) + l3*sin(theta3) + l4*sin(theta4) - yc;
end